{
 "cells": [
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "# 机械臂标定"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "## 导入依赖"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 1,
   "metadata": {},
   "outputs": [],
   "source": [
    "import math\n",
    "# 阿凯机器人工具箱\n",
    "from kyle_robot_toolbox.robot_arm.arm5dof_uservo import Arm5DoFUServo"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "## 机械臂初始化"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 2,
   "metadata": {},
   "outputs": [],
   "source": [
    "# 创建机械臂\n",
    "arm = Arm5DoFUServo(config_folder=\"./config\", \\\n",
    "                    is_init_pose=False)"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "## 设置舵机为阻尼模式"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 3,
   "metadata": {},
   "outputs": [],
   "source": [
    "# 设置为阻尼模式\n",
    "# 数值越大，阻尼越大\n",
    "arm.set_damping(1000) "
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "## 样本采集"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "将机械臂掰直, 水平于桌面, 夹爪张开。 采集舵机原始角度"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "![](image/pose1.jpg)"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 4,
   "metadata": {},
   "outputs": [
    {
     "name": "stdout",
     "output_type": "stream",
     "text": [
      "J1: -7.3 J2: -0.1 J3: -89.9 J4: 92.0 J5: 2.0 J6: 89.8\n"
     ]
    }
   ],
   "source": [
    "pose1_servo_raw_angle = arm.get_servo_angle_list()\n",
    "# 打印舵机原始角度\n",
    "j1_0, j2_n90, j3_90, j4_n90, j5_0, j6_90 = pose1_servo_raw_angle\n",
    "print(f\"J1: {j1_0:.1f} J2: {j2_n90:.1f} J3: {j3_90:.1f} J4: {j4_n90:.1f} J5: {j5_0:.1f} J6: {j6_90:.1f}\")    "
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "夹爪闭合, 令夹爪运动到机械臂左侧，夹爪末端垂直于桌面，且连杆之间均为90度直角。\n",
    "\n",
    "![](./image/pose2.jpg)"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 5,
   "metadata": {},
   "outputs": [
    {
     "name": "stdout",
     "output_type": "stream",
     "text": [
      "J1: -97.9 J2: 95.9 J3: 7.0 J4: 3.0 J5: -94.9 J6: 1.6\n"
     ]
    }
   ],
   "source": [
    "pose2_servo_raw_angle = arm.get_servo_angle_list()\n",
    "# 打印舵机原始角度\n",
    "j1_90, j2_0, j3_0, j4_0, j5_90, j6_0 = pose2_servo_raw_angle\n",
    "print(f\"J1: {j1_90:.1f} J2: {j2_0:.1f} J3: {j3_0:.1f} J4: {j4_0:.1f} J5: {j5_90:.1f} J6: {j6_0:.1f}\")    "
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "## 标定舵机"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "计算各个关节的比例系数与角度偏移量. 用一个简单的一次函数来表示舵机原始角度与机械臂关节弧度之间的关系"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "$$\n",
    "angle_i = k_i*\\theta_{i} + b_i\n",
    "$$\n",
    "\n",
    "* $\\theta_i$关节弧度\n",
    "* $angle_i$ 舵机原始角度\n",
    "* $k_i$ 比例系数\n",
    "* $b_i$ 偏移量"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 6,
   "metadata": {},
   "outputs": [],
   "source": [
    "def calc_kb(angle_a, angle_b, theta_a, theta_b):\n",
    "    k = (angle_a-angle_b) / (theta_a-theta_b)\n",
    "    b = angle_a - k*theta_a\n",
    "    return k, b"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 7,
   "metadata": {},
   "outputs": [
    {
     "name": "stdout",
     "output_type": "stream",
     "text": [
      "joint2servo:\n",
      "  k: [-57.678, 61.115, -61.688, -56.659, -61.688, 56.150]\n",
      "  b: [-7.300, 95.900, 7.000, 3.000, 2.000, 1.600]\n"
     ]
    }
   ],
   "source": [
    "k1, b1 = calc_kb(j1_0, j1_90, 0, math.pi/2)\n",
    "k2, b2 = calc_kb(j2_n90, j2_0, -math.pi/2, 0)\n",
    "k3, b3 = calc_kb(j3_90, j3_0, math.pi/2, 0)\n",
    "k4, b4 = calc_kb(j4_n90, j4_0, -math.pi/2, 0)\n",
    "k5, b5 = calc_kb(j5_0, j5_90, 0, math.pi/2)\n",
    "k6, b6 = calc_kb(j6_90, j6_0, math.pi/2, 0)\n",
    "\n",
    "print(\"joint2servo:\")\n",
    "print(f'  k: [{k1:.3f}, {k2:.3f}, {k3:.3f}, {k4:.3f}, {k5:.3f}, {k6:.3f}]')\n",
    "print(f\"  b: [{b1:.3f}, {b2:.3f}, {b3:.3f}, {b4:.3f}, {b5:.3f}, {b6:.3f}]\")"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "将打印出来的字符串替换掉`config/arm.yaml`里面的对应配置项。 "
   ]
  }
 ],
 "metadata": {
  "kernelspec": {
   "display_name": "Python 3 (ipykernel)",
   "language": "python",
   "name": "python3"
  },
  "language_info": {
   "codemirror_mode": {
    "name": "ipython",
    "version": 3
   },
   "file_extension": ".py",
   "mimetype": "text/x-python",
   "name": "python",
   "nbconvert_exporter": "python",
   "pygments_lexer": "ipython3",
   "version": "3.8.10"
  }
 },
 "nbformat": 4,
 "nbformat_minor": 2
}
